function [simPoints, f,tt] = correct_electrodes(points,voxelSize,centroid,out,P)

% Principal component analysis
[coeff,score] = princomp(points);

meanX = mean(points,1);
[n,p] = size(points);
dirVect = coeff(:,1);
Xfit1 = repmat(meanX,n,1) + score(:,1)*coeff(:,1)';
t = [min(score(:,1)), max(score(:,1))];
endpts = [meanX + t(1)*dirVect'; meanX + t(2)*dirVect'];


%% check order
if exist('centroid')
    centrdist = distancePoint(centroid,endpts);
    [mdist i] = min(centrdist);
    
    if i == 2
        endpts = endpts(end:-1:1,:);
        dirVect = -dirVect;
        Xfit1 = Xfit1(end:-1:1,:);
    end
end

%% Tune length 
X = [0 0 0;0 0 1; 0 1 0;0 1 1;1 0 0; 1 0 1; 1 1 0; 1 1 1 ];

X1 = fliplr(X);

normS =norm(diff(endpts));
step = 0.1;
pp = (-5:step:normS*1.5)';
% global Signal
clear ff
starttrig=0;
for j=1:length(pp)
    
    point = endpts(1,:) + pp(j)*dirVect';
    pointco = max(floor(point),1);
    mm = P(pointco(1):pointco(1)+1,pointco(2):pointco(2)+1,pointco(3):pointco(3)+1);
    mm = double(mm(:));
    
    y = abs(pointco-point);
    p = prod(1-abs(X1-repmat(y,length(X1),1)),2);
    
    ss = p.*mm;
    SS(j) = sum(ss);
    
    if starttrig==0
        if SS(j) > 0.05
            starttrig = 1;
        end
    elseif(SS(j) < 0.05)
        break;
    end
end
% Signal{end+1} = SS;


rDirVec = voxelSize'.*dirVect;
rDirVec = rDirVec./sum(rDirVec.^2)^0.5;

wprim = norm(3.5*rDirVec'*diag(1./voxelSize));
wprim = 2*pi*(1/wprim)*step;

[pmaxes, differ] = finetune(SS,wprim);

if ~isempty(pmaxes)
    pmaxes = (pmaxes-5/step)*step;
    
    fitPoints = repmat(pmaxes,3,1)'*diag(dirVect);
    simPoints = repmat(endpts(1,:),size(fitPoints,1),1)+fitPoints;
    
    % dist = dist*step;
    differ = 2*pi/differ*step;
else
    simPoints = [];
    return;
end
    


% 
% %% Calculate simpoints - in absalute coordinats, fitpoints - relative
% %% coordinates
% 
% 
% a = norm(voxelSize'.*dirVect*differ);
% % title(num2str(a))
% 
% rendp = endpts*diag(voxelSize);
% dist = norm(rendp(1,:) - rendp(2,:));
% tp = 0:norm(voxelSize'.*dirVect*differ):norm(voxelSize'.*dirVect*dist);
% % tp = 0:3.5:norm(voxelSize'.*dirVect*dist);
% 
% 
% rfitPoints = repmat(tp,3,1)'*diag(rDirVec);
% rsimPoints = repmat(rendp(1,:),size(rfitPoints,1),1)+rfitPoints;
% fitPoints = rfitPoints*diag(1./voxelSize);
% simPoints = repmat(endpts(1,:),size(fitPoints,1),1)+fitPoints;
% 
% %% Tune position
% 
% tt = -differ/2:0.05:differ/2;
% f = zeros(1,length(tt));
% ttt = 1;
% for j=tt;
%     
%     nend = endpts(1,:)+j*dirVect';
%     simPoints = repmat(nend(1,:),size(fitPoints,1),1)+fitPoints;
%     sum1 = 0;
%     
%     temp = zeros(size(simPoints,1),2)*NaN;
%     for i=1:size(simPoints,1)
%         
%         d = distancePoint(simPoints(i,:),Xfit1);
%         [ds indd] = sort(d);
%         sum1 = sum1 + ds(1);
%         
%     end
%     
%     f(ttt) = sum1;
%     ttt = ttt+1;
% end
% 
% % plot(tt,f,'LineWidth',3)
% 
% % figure
% [v,i] = min(f);
% nend = endpts(1,:)+tt(i)*dirVect';
% 
% simPoints = repmat(nend(1,:),size(fitPoints,1),1)+fitPoints;

if exist('out')
    if(strcmp(out,'mm'))
        simPoints = simPoints*diag(voxelSize);
    end
end
% figure
% plot3(simPoints(:,1),simPoints(:,2),simPoints(:,3),'rx','MarkerSize',14)
% hold
% plot3(Xfit1(:,1),Xfit1(:,2),Xfit1(:,3),'bo','MarkerSize',20)



end

function d = distancePoint(p1,x0)

D = x0 - repmat(p1,size(x0,1),1);

d = (D(:,1).^2+D(:,2).^2+D(:,3).^2).^0.5;

end

function [pmaxes, differ] = finetune(h,wprim)

h = smooth(h,10)';
[mins maxes] = minmaxes(h);


tmins = mins;

% In right direction
[mins maxes] = findpoints(mins,maxes,h);
mins = mins(end:-1:1);
maxes = maxes(end:-1:1);
% In left direction
[mins maxes] = findpoints(mins,maxes,h);
mins = mins(end:-1:1);
maxes = maxes(end:-1:1);


mins = mins(h(mins)<mean(h(maxes)));

maxes = maxes(h(maxes)>mean(h(mins)));

tind = (tmins>mins(end));

lastmaxes = maxes(maxes < mins(end));
firstlastmaxes = lastmaxes(abs(h(lastmaxes)-mean(h(mins))) > abs(h(lastmaxes)-mean(h(maxes))));

h1 = h(maxes(1)-round(maxes(1)/2):min([firstlastmaxes(end)+round(maxes(1)/2) length(h)]));

[differ cdf] = fitfourier(h1,wprim);
[pmins pmaxes] = minmaxes(cdf);

if isempty(pmins) || isempty(pmaxes)
    return;
end

pmaxes = pmaxes+(maxes(1)-round(maxes(1)/2))-1;


% first check of le vis
flag = 0;
endmin2 = (mins(end)+round(2*pi/differ));

if length(h)>endmin2
    if(abs(h(endmin2)-h(mins(end))) > abs(h(endmin2)-h(maxes(end))))
        flag = 1;
    end
end

% second check of the vis
if (mean(h(tmins(tind))) > h(mins(end))) || flag
%     dist = (mins(end))-maxes(1);
else
%     dist = (mins(end)+avgdiff*2)-maxes(1);
      pmaxes(end+1) = pmaxes(end)+round(2*pi/differ);
end

elcount = [5 8 10 15 18];

if length(pmaxes) < elcount(1)
    pmaxes = [];
end

if length(pmaxes) > elcount(end)
    pmaxes = pmaxes(1:elcount(end));
end
eldiff = elcount-length(pmaxes);
if ~isempty(eldiff(eldiff==1))
    pmaxes(end+1) = pmaxes(end)+round(2*pi/differ);
end


% dmaxes = maxes(maxes < mins(end));

% d = dmaxes(end) - dmaxes(1);
% differ = d/(length(dmaxes)-1);
% figure
% plot(h)
% hold
% plot(mins,h(mins),'ro')
% plot(maxes,h(maxes),'go')
% plot(firstlastmaxes,h(firstlastmaxes),'y*');
% 
% plot([1 length(h)],[mean(h(maxes)) mean(h(maxes))],'k')
% plot([1 length(h)],[mean(h(mins)) mean(h(mins))],'m')
end

function [mins maxes avgdiff] = findpoints(mins,maxes,h)
diffmm = abs(maxes(1:min([length(maxes) length(mins)]))-mins(1:min([length(maxes) length(mins)])));
diffvmm = abs(h(maxes(1:min([length(maxes) length(mins)])))-h(mins(1:min([length(maxes) length(mins)]))));

sortdiffmm = sort(diffmm,'descend');
sortdiffvmm = sort(diffvmm,'descend');

avgdiff = mean(sortdiffmm(2:floor(end/2)));
avgvdiff = mean(sortdiffvmm(2:floor(end/2)));

[v ind] = find(diffmm<avgdiff*0.50);
[v indv] = find(diffvmm<avgvdiff*0.40);

maxes(unique([ind indv])) = [];
mins(unique([ind indv])) = [];
end

function [w cfd] = fitfourier(h1,wprim)

% options = fitoptions('fourier1');
g = fittype('fourier1');

len = length(h1);
W(1) = 0;
for i = 1:(len/2)/50

    cf = fit((1:length(h1)-50*(i-1))',h1(1:end-50*(i-1))',g);
    W(i+1) = cf.w;
    
    if(abs(W(end-1)-W(end)) < 0.001 && abs(W(end)-wprim) < W(end)*0.1)
        break;
    end
    
end

cfd = cf(1:length(h1));
w = mean(W(end-1:end));
% figure;
% plot(h1)
% hold
% plot(cfd,'r')
end

function [mins maxes] = minmaxes(h)

d = diff(h);
maxmin = [];
for i=1:length(h)-2
    if d(i)==0
        maxmin = [maxmin, i];
    elseif sign(d(i))~=sign(d(i+1))
        maxmin = [maxmin, i+1];
    end
end

if length(maxmin) < 2
    mins = [];
    maxes = [];
    return;
elseif h(maxmin(1))<h(maxmin(2))
    mins = maxmin(1:2:length(maxmin));
    maxes  = maxmin(2:2:length(maxmin));
else
    mins = maxmin(2:2:length(maxmin));
    maxes  = maxmin(1:2:length(maxmin));
end

end